/*
 * PID.h
 *
 *  Created on: Jun 10, 2024
 *      Author: Liangmiyi
 */
#ifndef My_INC_PHY_PHY_PID_H_
#define My_INC_PHY_PHY_PID_H_
#include <stdint.h>
#include <stdio.h>

// 定义是否使用 PID 的 Kd 参数
#define PID_KD_EN  1U
#define PID_KD_DIS 0U
#define PID_KD     PID_KD_DIS

// PI控制器结构体
typedef struct
{
    // 参数
    float kp;  // 比例系数
    float ki;  // 积分系数
    float kd;  // 微分系数
    float max; // 输出上限
    float min; // 输出下限
    float kaw; // 抗饱和系数

    // 状态变量
    float IntegralIterm;       // 积分项累加值
    float Differential;        // 微分项
    float lastError;           // 上一次误差
    float currentError;        // 当前误差
    float out;                 // 输出值
    float antiWindupError;     // 当前抗饱和误差
    float antiWindupLastError; // 上一次抗饱和误差
} AntiWindupPID__Typedef;

// 初始化PI控制器
void AntiWindup_PID_Init(AntiWindupPID__Typedef *pid, float Kp, float Ki, const float ts, float Kd, float min,
                         float max);

// 重置PI控制器
void AntiWindup_PID_Reset(AntiWindupPID__Typedef *pid);

// 执行PI计算
 static inline float AntiWindup_PID_Control_Inline(AntiWindupPID__Typedef *pid, float Target, float Actual)
{

    pid->currentError = Target - Actual;
    pid->IntegralIterm += pid->ki * (pid->currentError + pid->lastError);
    // pid->Differential = pid->kd * (pid->currentError - pid->lastError);
    pid->lastError = pid->currentError;

    if (pid->out > pid->max)
        pid->antiWindupError = pid->max - pid->out;
    else if (pid->out < pid->min)
        pid->antiWindupError = pid->min - pid->out;
    else
        pid->antiWindupError = 0.0f;


    pid->IntegralIterm += pid->kaw * (pid->antiWindupError + pid->antiWindupLastError);
    pid->antiWindupLastError = pid->antiWindupError;
    // pid->out = (pid->kp * pid->currentError) + pid->IntegralIterm + pid->Differential;
    pid->out = (pid->kp * pid->currentError) + pid->IntegralIterm;

    return pid->out;
}
//static inline float AntiWindup_PID_Control_Inline(AntiWindupPID__Typedef *pid, float Target, float Actual)
//{
//    pid->currentError = Target - Actual;
//    if (pid->out > pid->max)
//    {
//        if (pid->currentError < 0)
//            pid->IntegralIterm = pid->IntegralIterm + pid->ki * (pid->currentError + pid->lastError);
//    }
//
//    else if (pid->out < pid->min)
//    {
//        if (pid->currentError > 0)
//            pid->IntegralIterm = pid->IntegralIterm + pid->ki * (pid->currentError + pid->lastError);
//    }
//    else
//    {
//        pid->IntegralIterm = pid->IntegralIterm + pid->ki * (pid->currentError + pid->lastError);
//    }
//    pid->out = (pid->kp * pid->currentError) + pid->IntegralIterm;
//    pid->lastError = pid->currentError;
//    return pid->out;
//}

typedef struct
{
    float A0;         // 系数
    float B0;         // 系数
    float ERROR;      // 当前误差
    float ERROR_Last; // 前一次误差
    float Out;        // 误差累加
    float output_min; // 输出最小值
    float output_max; // 输出最大值
} IncrementalPID_Typedef;
void                Increasing_PI_Init(IncrementalPID_Typedef *PI, float Kp, float Ki, const float ts, float output_min,
                                       float output_max);
static inline float Increasing_PI_Control_Inline(IncrementalPID_Typedef *PI, float TARGET, float ACTUAL)
{
    // 计算当前误差
    PI->ERROR = TARGET - ACTUAL;
    PI->Out += (PI->A0 * PI->ERROR) + (PI->B0 * PI->ERROR_Last);
    // 限制输出
    PI->Out = (PI->Out > PI->output_max) ? PI->output_max : PI->Out;
    PI->Out = (PI->Out < PI->output_min) ? PI->output_min : PI->Out;
    PI->ERROR_Last = PI->ERROR;
    return PI->Out;
}

typedef struct
{
    float Coeff_B0; // 系数B0
    float Coeff_B1; // 系数B1
    float Yn_1;
    float Xn_1;

    float output_min; // 输出最小值
    float output_max; // 输出最大值
} PI_Typedef;
void                PI_Init(PI_Typedef *PI, float Kp, float Ki, float ts, float output_min, float output_max);
static inline float PI_Control_Inline(PI_Typedef *PI, float TARGET, float ACTUAL)
{
    // 计算当前误差
    float Xn = TARGET - ACTUAL;
    float Yn = PI->Coeff_B0 * Xn + PI->Coeff_B1 * PI->Xn_1 + PI->Yn_1;

    // 限制输出
    Yn = (Yn > PI->output_max) ? PI->output_max : Yn;
    Yn = (Yn < PI->output_min) ? PI->output_min : Yn;

    PI->Yn_1 = Yn; // 更新上一次输出
    PI->Xn_1 = Xn; // 更新上一次输入
    return Yn;
}

#endif /* INC_PID_H_ */
